#include "wifibot.h"

namespace Lio {

Wifibotsc12::Wifibotsc12():control(FALSE)
{
	send.set(this, "WifibotSend");
	receive.set(this, "WifibotReceive");
}

Wifibotsc12::~Wifibotsc12()
{
	Disconnect();
}

BOOL Wifibotsc12::Connect(const char * ip, int port)
{
	Lock lock(&cs_connect);
	// Create a socket connection and a new process for listening the communication:
	threadRun=TRUE;
	SetComControl(0,0);
	// Thread startup setup:
	if(socket.connect(ip, port))
	{
		start(&send);
		start(&receive);
		timer.restart();
		return TRUE;
	}
	return FALSE;
}

void Wifibotsc12::Disconnect()
{
	socket.disconnect();
	threadRun=FALSE;
}

DWORD Wifibotsc12::run(const ThreadInfo *info)
{
	if(info == &receive)
	{
		PRINT("Receiving started...");
		Lock lock(&cs_receive);
		WifibotReceive *rcvtmp;
		while(threadRun)
		{
			// Clear the Buffer:
			ClearMemory(&rcvbuf,7);
			// Wait To Receive 7 Bytes in the buffer
			if (socket.receive(&rcvbuf,7))
			{
				// Synchronize start:
				Lock lock(&cs);
				// Append new data buffer in the waiting list
				rcvtmp=rcvList.add();
				// Copy the received information in the data buffer:
				memcpy(rcvtmp->data,rcvbuf,7);
				rcvtmp->period=timer.restart();
				// Synchronize end:
				
			}
			Sleep(RCV_THREAD_SLEEP);
		}
		PRINT("Receiving stoped.");
	}
	else if(info == &send)
	{
		PRINT("Sending started...");
		Lock lock(&cs_send);
		while(threadRun)
		{
			socket.send(&sendbuf, 2);
			Sleep(SEND_THREAD_SLEEP);
		}
		PRINT("Sending stoped.");
	}
	return 0;
}

void Wifibotsc12::ConvertData(const WifibotReceive *rcvbuf, SensorData *RobotSensors)
{
	// Format the temp buffer in the Sensor Struct:
	RobotSensors->BatVoltage=NORM_VOLT(rcvbuf->data[0]);
	RobotSensors->SpeedFrontLeft=SECT_TO_RPM(rcvbuf->data[1]);
	RobotSensors->SpeedRearLeft=SECT_TO_RPM(rcvbuf->data[2]);
	RobotSensors->SpeedFrontRight=SECT_TO_RPM(rcvbuf->data[3]);
	RobotSensors->SpeedRearRight=SECT_TO_RPM(rcvbuf->data[4]);
	RobotSensors->IRLeft=rcvbuf->data[5];
	RobotSensors->IRRight=rcvbuf->data[6];
	RobotSensors->DistLeft=getDist(rcvbuf->data[5],IR_LEFT);
	RobotSensors->DistRight=getDist(rcvbuf->data[6],IR_RIGHT);
	RobotSensors->Timer=rcvbuf->period;
}
BOOL Wifibotsc12::GetNextData(WifibotReceive *rcvbuf)
{
	if(!rcvList.first)
		return FALSE;
	// Synchronize start:
	Lock lock(&cs);
	// Get the first data in the waiting list:
	WifibotReceive *rcvtmp=&(rcvList.first->value);
	// Copy the data in the output buffer
	memcpy(rcvbuf->data,rcvtmp->data,7);
	rcvbuf->period=rcvtmp->period;
	// Remove the data from the waiting list
	rcvList.remove(rcvList.first);
	// Synchronize end:
	return TRUE;
}
// Send Low Level Control For Each Wheel
BOOL Wifibotsc12::SetComControl(BYTE coml, BYTE comr)
{
	sendbuf[0]=coml;
    sendbuf[1]=comr;
	return TRUE;
}
//Control Based on Rotation and Translation Speed, Based on Two Wheel Model for the Wifibot
void Wifibotsc12::SpeedToWheelControl(double Vrot, double Vlin, double * left, double * right)
{
	*right=Vlin+Vrot;
	*left=Vlin-Vrot;
}
void Wifibotsc12::WheelToSpeedControl(double left, double right, double * Vrot, double * Vlin)
{
	*Vlin=0.5*(right+left);
	*Vrot=0.5*(right-left);
}
// Control To Each Wheel (-1 to 1)
double Wifibotsc12::WheelToComControl(double left, double right, BYTE * coml, BYTE * comr)
{
	int maxv=MAX_SPEED_FREE,dirl=DIR_FORWARD,dirr=DIR_FORWARD,ctrl=MODE_FREE;
	double reduct=1.0,absl=ABS(left),absr=ABS(right);
	// normalization (-1 to 1)
	if(absl>1.0||absr>1.0)
	{
		reduct/=max(absl,absr);
		if(reduct<1)
		{
			absl*=reduct;
			absr*=reduct;
		}
	}
	if(left<0)
		dirl=DIR_BACKWARD;
	if(right<0)
		dirr=DIR_BACKWARD;
	if (control)
	{
		ctrl=MODE_CTRL;
		maxv=MAX_SPEED_CTRL;
	}
	*coml=ctrl+dirl+int(absl*maxv);
	*comr=ctrl+dirr+int(absr*maxv);
	return reduct;
}
// Send Control Based on Joystick
void Wifibotsc12::JoyToComControl(int x, int y, BYTE * coml, BYTE * comr)
{
	int maxv,vr,vl,ctrl;

	if (control)
	{
		ctrl=MODE_CTRL;
		maxv=MAX_SPEED_CTRL;
	}
	else
	{
		ctrl=MODE_FREE;
		maxv=MAX_SPEED_FREE;
	}
	if(y>=0)
	{
		vl=abs(x+y);
		vr=abs(x-y);
	}
	else
	{
		vl=abs(x-y);
		vr=abs(x+y);
	}
	if (y>0)
	{	
		if (x>=0) 
		{
			if(vl>maxv)
				vl=maxv;
			if(x>y)
				vr=0;
		}
		else 
		{
			if(abs(x)>y)
				vl=0;
			if(vr>maxv)
				vr=maxv;
		}
		*coml=ctrl+DIR_FORWARD+vl;
		*comr=ctrl+DIR_FORWARD+vr;
	}	
	else if (y<0)
	{
		if (x>=0) 
		{
			if(vl>maxv)
				vl=maxv;
			if(x>abs(y))
				vr=0;		
		}
		else 
		{
			if(vr>maxv)
				vr=maxv;
			if(x<y)
				vl=0;		
		}
		*coml=ctrl+DIR_BACKWARD+vl;
		*comr=ctrl+DIR_BACKWARD+vr; 
	}
	else 
	{
		if ((y==0)&&(x<0))
		{
			*coml=ctrl+DIR_BACKWARD+vl;
			*comr=ctrl+DIR_FORWARD+vr;
		} 			//Gpivot
		if ((y==0)&&(x>0))
		{
			*coml=ctrl+DIR_FORWARD+vl;
			*comr=ctrl+DIR_BACKWARD+vr;
		}			//Dpivot
		if ((x==0)&&(y==0))
		{
			*coml=0;
			*comr=0;
		}
	}
}

BOOL WifibotApplication::create ()		
{
	FlatApplication::create();
	// name, size and color depth for the window:
	name=_T("Wifibot Application");
	display.width=800;
	display.height=600;
	display.bpp=32;
	// Number Format Precision
	format.precision=3;
	clearColor=0xFFEEEEEE;

	fontSmall.height=-12;
	fontSmall.setFace(_T("Courier New"));

	fontBold.bold=TRUE;
	fontBold.height=-18;
	fontBold.setFace(_T("Arial"));

	space=FALSE;
	coml=comr=0;

	// Control Setup
	bConnect.text=_T("Connect");
	bConnect.borderLine=1;
	bConnect.borderColor=INT_DGRAY;
	bConnect.activeLine=2;
	bConnect.font=&font;
	bConnect.fontColor=INT_BLACK;
	bConnect.margin.set(10,3);
	bConnect.bgColor=0xFFDDDDDD;
	bConnect.overColor=INT_WHITE;
	bConnect.pressColor=INT_YELLOW;
	bConnect.location.set(10,10);
	bConnect.adjustSize=TRUE;
	bConnect.checkable=TRUE;

	tbIP.text=DEFAULT_IP;
	tbIP.borderLine=1;
	tbIP.borderColor=INT_BLACK;
	tbIP.activeLine=2;
	tbIP.font=&font;
	tbIP.fontColor=INT_BLACK;
	tbIP.margin.set(2,1);
	tbIP.stick(DIR_RIGHT,5,0);

	tbPort.copy(tbIP);
	tbPort.text=DEFAULT_PORT;
	tbPort.stick(DIR_RIGHT,5,0);
	tbPort.size.x=80;

	lSource.text=_T("Source:");
	lSource.fontColor=INT_BLACK;
	lSource.margin.set(5,1);
	lSource.adjustSize=TRUE;
	lSource.stick(DIR_RIGHT,5,0);

	//chSource.align=DIR_LEFT;
	chSource.margin.set(10,1);
	chSource.bgColor=INT_WHITE;
	chSource.fontColor=INT_BLACK;
	chSource.add(_T("Wifibot"),SOURCE_WIFIBOT);
	chSource.add(_T("File"),SOURCE_FILE);
	chSource.setSelectedIndex(SOURCE_WIFIBOT);
	chSource.stick(DIR_RIGHT);

	bSave.text=_T("Save");
	bSave.copy(bConnect);
	bSave.stick(DIR_RIGHT,5,0);

	bMem.text=_T("Buffer");
	bMem.copy(bConnect);
	bMem.checkable=FALSE;
	bMem.stick(DIR_RIGHT,5,0);

	tbMem.copy(tbIP);
	tbMem.size.x=50;
	tbMem.stick(DIR_RIGHT,5,0);

	bExit.text=_T("Exit");
	bExit.copy(bConnect);
	bExit.pressColor=INT_WRED;
	bExit.stick(DIR_RIGHT,5,0);
	bExit.checkable=FALSE;

	bFilter.copy(bMem);
	bFilter.text=_T("Filter");
	bFilter.stick(DIR_DOWN,0,10,&bConnect);

	lOrder.copy(lSource);
	lOrder.text=_T("Order:");
	lOrder.stick(DIR_RIGHT,5,0);

	tbOrder.copy(tbPort);
	tbOrder.stick(DIR_RIGHT,5,0);
	
	lWindow.copy(lSource);
	lWindow.text=_T("Window:");
	lWindow.stick(DIR_RIGHT,5,0);
	
	lCutOff.copy(lSource);
	lCutOff.text=_T("CutOff:");
	lCutOff.stick(DIR_RIGHT,5,0);
	
	tbCutOff.copy(tbPort);
	tbCutOff.stick(DIR_RIGHT,5,0);

	chWindow.copy(chSource);
	chWindow.add(_T("RECT"));
	chWindow.add(_T("BARTLETT"));
	chWindow.add(_T("VONHANN"));
	chWindow.add(_T("HAMMING"));
	chWindow.add(_T("BLACKMAN"));
	chWindow.add(_T("BLACKMAN_HARRIS_3"));
	chWindow.add(_T("BLACKMAN_HARRIS_4"));
	chWindow.add(_T("HARRIS_NUTALL_3"));
	chWindow.add(_T("HARRIS_NUTALL_4"));
	chWindow.stick(DIR_RIGHT,5,0);

	fgIR.bgColor=INT_GRAY;
	fgIR.graph.clear();
	fgIR.graph.smooth=TRUE;
	fgIR.graph.grid.set(3,5);
	fgIR.graph.margin=10;
	fgIR.graph.marginChannel.set(15,50);
	fgIR.graph.font=&fontSmall;
	fgIR.graph.formatX.precision=3;
	fgIR.graph.formatY.precision=3;
	fgIR.margin.set(10,10);
	fgIR.font=&fontSmall;
	fgIR.fontColor=INT_BLACK;
	fgIR.location.set(10,GRAPH_POS);
	fgIR.graph.skip=FALSE;
	fgIR.toolbox=FALSE;
	chnlsIR[CHNL_TIME]=fgIR.graph.addChannel(_T("t"),NULL,INT_BLACK);
	chnlsIR[CHNL_IRL]=fgIR.graph.addChannel(_T("IRL(t)"),_T("t"),INT_BLUE);
	chnlsIR[CHNL_IRR]=fgIR.graph.addChannel(_T("IRR(t)"),_T("t"),INT_RED);
	chnlsIR[CHNL_FIRL]=fgIR.graph.addChannel(_T("FIRL(t)"),_T("t"),INT_DBLUE,0,NULL,2);
	chnlsIR[CHNL_FIRR]=fgIR.graph.addChannel(_T("FIRR(t)"),_T("t"),INT_DRED,0,NULL,2);
	
	// Include the controls in the window
	
	add(&bConnect);
	add(&tbIP);
	add(&tbPort);
	add(&lSource);
	add(&chSource);
	add(&bSave);
	add(&bMem);
	add(&tbMem);
	add(&bExit);
	add(&bFilter);
	add(&lOrder);
	add(&tbOrder);
	add(&lWindow);
	add(&chWindow);
	add(&lCutOff);
	add(&tbCutOff);
	add(&fgIR);

	createAction();

	setBufferSize(DEF_BUFFER_SIZE);
	filterOrder=0;
	filterOK=FALSE;
	setFilter(DEF_FIR_ORDER,FIR_WIN_RECT,DEF_CUTOFF_FREQ);

	start( &operate );
	
	return TRUE;
}
void WifibotApplication::setFilter(int order, int window, double cutOff)
{
	if(order)
	{
		if(order<0)
			order=buffSize-1;
		if(order%2==0)
			order++;	
	}
	if(cutOff<0)
		cutOff=0;
	if(window<0||window>=FIR_WIN_COUNT)
		window=FIR_WIN_RECT;
	tbOrder.setText(format.toString(order));
	chWindow.setSelectedIndex(window);
	tbCutOff.setText(format.toString(cutOff));
	filterOrder=order;
	windowType=window;
	cutOffFreq=cutOff;
	filter.setSize(order);
}
BOOL WifibotApplication::glCreate ()
{
	fontSmall.create(window.hDC);
	fontBold.create(window.hDC);
	FlatApplication::glCreate();
	return TRUE;
}
void WifibotApplication::glDestroy()
{
	FlatApplication::glDestroy();
	fontSmall.destroy();
	fontBold.destroy();
}
void WifibotApplication::reshape()
{
	FlatApplication::reshape();
	fgIR.size.set(float(0.5*window.width-15),float(window.height-GRAPH_POS-10));
	reshapeAction();
	container.build();
}
BOOL WifibotApplication::listen(WORD id, WORD code, const void *info)
{
	if(id==bConnect.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			switch(chSource.getSelectedIndex())
			{
			case SOURCE_WIFIBOT:
				if(bConnect.checked)
				{
					// Connect to Wifibot:
					if(	!start(&connect) )// Connect to Wifibot // Start the communication by sending some data
						bConnect.checked=FALSE;
					else
						bConnect.disabled=TRUE;
				}
				else
				{
					// Disconnect from Wifibot:
					robot.Disconnect();
				}
				break;
			case SOURCE_FILE:
				if(bConnect.checked)
				{
					// Open file
					if(	!srcFileIn.open(SRC_FILE_IN_NAME,_T("rb"))) // Start the communication by sending some data
						bConnect.checked=FALSE;
				}
				else
				{
					// Close file:
					srcFileIn.close();
				}
				break;
			}
			time=0;
			if(bConnect.checked)
				fgIR.graph.clear(TRUE);
			connectAction();
			return TRUE;
		}
	}
	else if(id==bSave.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			if(bSave.checked)
				srcFileOut.open(SRC_FILE_OUT_NAME,_T("wb"));
			else
				srcFileOut.close();
			return TRUE;
		}
	}
	else if(id==bMem.id||id==tbMem.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setBufferSize(format.toInt(tbMem.getText(),DEF_BUFFER_SIZE));
		}
	}
	else if(id==bExit.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			terminate();
			return TRUE;
		}
	}
	else if(	id==bFilter.id||
				id==tbOrder.id||
				id==chWindow.id||
				id==tbCutOff.id)
	{
		if(code==FLAT_ACTION_DEFAULT)
		{
			setFilter(	format.toInt(tbOrder.getText()),
						chWindow.getSelectedIndex(),
						format.toDouble(tbCutOff.getText()));
			return TRUE;
		}
	}
	return listenAction(id,code,info);
}
DWORD WifibotApplication::run(const ThreadInfo *info)
{
	if(info == &connect)
	{
		PRINT("Connecting started...");
		ip = tbIP.getText();
		port = format.toInt(tbPort.getText());
		bConnect.checked = robot.Connect(ip, port);
		bConnect.disabled=FALSE;
		PRINT("Connected: %d", bConnect.checked);
	}
	else if(info == &operate)
	{
		BOOL received=FALSE;
		double *buffL,*buffR;

		for(;;)
		{
			// Specify that the window shaw be redrawn
			setUpdated(TRUE);

			if(keyboard.keys[VK_SPACE])
			{
				if(!space)
				{
					space=TRUE;
					bConnect.action();
				}
			}
			else
			{
				space=FALSE;
			}
			// Get the saved sensor information:
			if(bConnect.checked)
			{
				switch(chSource.getSelectedIndex())
				{
				case SOURCE_WIFIBOT:
					received=robot.GetNextData(&rcvbuf);
					if(received&&bSave.checked)
					{
						if(!srcFileOut.write(&rcvbuf))
						{
							bConnect.checked=FALSE;
							srcFileOut.close();
						}
					}
					break;
				case SOURCE_FILE:
					received=srcFileIn.read(&rcvbuf);
					if(!received)
					{
						bConnect.checked=FALSE;
						srcFileIn.close();
					}
					break;
				}
				if(received)
				{
					robot.ConvertData(&rcvbuf,&robotSensors);
					T=buffTime.elems[buffSize-1]-buffTime.elems[0];
					fs=double(buffSize)/T;
					if(filterOrder)
					{
						skirtLength=getFIRSkirt(windowType,filterOrder,fs);
						filterOK=getFIRFilter(filter.elems,filterOrder,LOWPASS,windowType,fs,cutOffFreq);
					}
					else
					{
						filterOK=FALSE;
					}
					shift();
					buffL=&(buffIRL.elems[buffSize-filterOrder]);
					buffR=&(buffIRR.elems[buffSize-filterOrder]);
					time+=robotSensors.Timer;
					buffTime.elems[buffSize-1]=time;
					buffIRL.elems[buffSize-1]=robotSensors.IRLeft;
					buffIRR.elems[buffSize-1]=robotSensors.IRRight;
					if(filterOK)
					{
						buffFIRL.elems[buffSize-1]=conv(buffL,filter.elems,filterOrder);
						buffFIRR.elems[buffSize-1]=conv(buffR,filter.elems,filterOrder);
					}
					else
					{
						buffFIRL.elems[buffSize-1]=buffIRL.elems[buffSize-1];
						buffFIRR.elems[buffSize-1]=buffIRR.elems[buffSize-1];
					}
					fgIR.graph.update();
					receiveAction();
				}
			}
			// actions
			updateAction();
			// Send command to Wifibot
			if(bConnect.checked)
				robot.SetComControl(coml,comr);
			// Sleep for 60 ms:
			Sleep(60);
		}
	}
	return 0;
}
BOOL WifibotApplication::setBufferSize(int size)
{
	if(size>=0&&size!=buffSize)
	{
		tbMem.setText(format.toString(size));
		buffSize=size;
		buffIRL.setSize(size,TRUE);
		buffIRR.setSize(size,TRUE);
		buffTime.setSize(size,TRUE);
		buffFIRL.setSize(size,TRUE);
		buffFIRR.setSize(size,TRUE);
		chnlsIR[CHNL_TIME]->setSource(buffTime);
		chnlsIR[CHNL_IRR]->setSource(buffIRR);
		chnlsIR[CHNL_IRL]->setSource(buffIRL);
		chnlsIR[CHNL_FIRR]->setSource(buffFIRR);
		chnlsIR[CHNL_FIRL]->setSource(buffFIRL);
		return TRUE;
	}
	return FALSE;
}
void WifibotApplication::shift()
{
	for(int i=0;i<buffSize-1;i++)
	{
		buffTime.elems[i]=buffTime.elems[i+1];
		buffIRL.elems[i]=buffIRL.elems[i+1];
		buffIRR.elems[i]=buffIRR.elems[i+1];
		buffFIRL.elems[i]=buffFIRL.elems[i+1];
		buffFIRR.elems[i]=buffFIRR.elems[i+1];
	}
}

// Constructor Initialization:
// 0.3 - wheel distance
// 1 - max speed
WifibotApplication::WifibotApplication()
{
	operate.set(this, "WifibotApplicationOperate");
	connect.set(this, "WifibotApplicationConnect");
}

} // namespace Lio